perm filename TRAJ.SAI[SYS,HE]3 blob
sn#014942 filedate 1972-12-01 generic text, type T, neo UTF8
00100 DEFINE WRITE="DEB_HAND";
00200 REQUIRE "VECTOR.SAI[SYS,HE]" SOURCE_FILE;
00300 EXTERNAL INTEGER _SKIP_;
00400 DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
00500 DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
00600 DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
00700 0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
00800 DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
00900 DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
01000 DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
01100 .5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
01200 REQUIRE "YELLOW.SAI[SYS,HE]" SOURCE_FILE;
01300 SAFE REAL ARRAY U1,T[0:101];
01400 SAFE REAL ARRAY DIAG[1:13];
01500 SAFE INTEGER ARRAY COEFF[0:'1037];
01550 SAFE REAL ARRAY ARRIVE_ARM,DEPART_ARM[1:4];
01575 REAL F1_ARM,F2_ARM,OBJECT_MASS,OBJECT_KXX,OBJECT_KYY,OBJECT_KZZ;
01587 INTEGER T1_ARM,T2_ARM;
01600 DEFINE MAX_SEG="8";
01700 SAFE REAL ARRAY POSITION[1:6,0:MAX_SEG];
01800 SAFE REAL ARRAY KOE[1:6,1:(MAX_SEG*3)+6];
01900 SAFE INTEGER ARRAY PERIOD[1:6,1:MAX_SEG];
02000 SAFE INTEGER ARRAY NS[1:6];
02100 SAFE REAL ARRAY POS[0:MAX_SEG];
02200 SAFE REAL ARRAY SOLN[1:(MAX_SEG*3)+6];
02300 SAFE INTEGER ARRAY TIM[1:MAX_SEG];
02400 SAFE STRING ARRAY BANDS[0:'37];
02500 SAFE INTEGER ARRAY TRACK[0:'40];
02600 INTEGER NEXT_BAND,BAND,BAND_NUMBER;
02700 PRELOAD_WITH 0,'1040,0;
02800 SAFE INTEGER ARRAY GOODIE[1:3];
02900 DEFINE UFBWRT="'707000000000";
03000 BOOLEAN FAST;
03100 SAFE REAL ARRAY LAST_ARM[1:6];
03200 SAFE REAL ARRAY LAST_TRANS[1:4,1:4];
03300 SAFE REAL ARRAY QA,D[0:16];
03400 SAFE REAL ARRAY TF[1:6];
03500 SAFE REAL ARRAY LU,NR[1:6,1:6];
03600 REAL DIGITS;
03700 INTEGER NNUL,LOOP,JOINT,DURATION;
03800 SAFE INTEGER ARRAY DATA_STACK[1:500];
03900 REQUIRE "MATRIX.REL[SYS,HE]" LOAD_MODULE;
04000 REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
04100 EXTERNAL SIMPLE PROCEDURE MMOVE(REFERENCE REAL R,A);
04200 EXTERNAL SIMPLE PROCEDURE MTIMES(REFERENCE REAL R,A,B);
04300 EXTERNAL REAL SIMPLE PROCEDURE TRACET(REFERENCE REAL X,Y);
04400 EXTERNAL SIMPLE PROCEDURE DECOMPOSE(INTEGER I; REAL ARRAY NR,LU);
04500 EXTERNAL SIMPLE PROCEDURE SOLVE(INTEGER I;REAL ARRAY NR,LU,DTH);
04600 EXTERNAL SIMPLE PROCEDURE IMPROVE(INTEGER I; REAL ARRAY NR,LU,DA,DTH;REFERENCE REAL DIGITS);
04700 INTEGER SIMPLE PROCEDURE SQAR (INTEGER I);
04800 RETURN ((I-1)*17);
04900
00100 SIMPLE PROCEDURE COMPLETA (INTEGER I;SAFE REAL ARRAY TH);
00200 BEGIN REAL S,C;
00300 INTEGER J,K;
00400 J←SQAR(I);
00500 K←6*(I-1);
00600 S←SIND(TH[I]);
00700 C←COSD(TH[I]);
00800 A[J]←C;
00900 A[J+1]←APAR[K]*S;
01000 A[J+2]←APAR[K+1]*S;
01100 A[J+3]←APAR[K+2]*C;
01200 A[J+4]←S;
01300 A[J+5]←APAR[K+3]*C;
01400 A[J+6]←APAR[K+4]*C;
01500 A[J+7]←APAR[K+5]*S;
01600 END;
01700
01800 SIMPLE PROCEDURE HANDPOS(SAFE REAL ARRAY TH);
01900 BEGIN INTEGER I;
02000 FOR I←1,2,4,5,6 DO COMPLETA(I,TH);
02100 A[SQAR(3)+11]←TH[3];
02200 A[3]←A[3]+SHOLDER[1];
02300 A[7]←A[7]+SHOLDER[2];
02400 MMOVE(A[0],A[0]);
02500 MMOVE(T[0],A[0]);
02600 UNDERFLOW(-1);
02700 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(T[SQAR(I)],T[SQAR(I-1)],A[SQAR(I)]);
02800 UNDERFLOW(0);
02900 END;
03000
03100 SIMPLE PROCEDURE UPDATE_SEG;
03200 BEGIN INTEGER I;
03300 HANDPOS(ARM_VECTOR);
03400 FOR I←3 STEP 1 UNTIL 6 DO
03500 BEGIN
03600 ARRBLT(ARM_LINK[I,1,1],T[SQAR(I)],16);
03700 END;
03800 GRASP←ARM_VECTOR[7];
03900 END;
04000
04100 SIMPLE PROCEDURE NOT_LESS(REFERENCE REAL V;REAL L);
04200 BEGIN
04300 IF V<0.0 ∧ V>-L THEN BEGIN V←0; RETURN END;
04400 IF V≥0.0 ∧ V< L THEN V←0;
04500 END;
04600
00100 SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZ);
00200 BEGIN PRELOAD_WITH 3,7,11;
00300 SAFE OWN INTEGER ARRAY KEY[1:6];
00400 PRELOAD_WITH [6] 0.0; SAFE OWN REAL ARRAY DIR[1:6];
00500 INTEGER I,J;
00600 REAL MAX,R;
00700 LABEL OUT;
00800 REDUCE(DX_DY_DZ);
00900 IF DX_DY_DZ[1]=0.0 ∧
01000 DX_DY_DZ[2]=0.0 ∧
01100 DX_DY_DZ[3]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←0.0;
01200 RETURN;
01300 END;
01400 UNDERFLOW(-1);
01500 ARRBLT(DIR[1],DX_DY_DZ[1],3);
01600 IF ABS(T[85+2])<ABS(T[85+6])
01700 THEN BEGIN KEY[4]←2;
01800 KEY[5]←IF ABS(T[85+6])<ABS(T[85+10]) THEN 6 ELSE 10
01900 END
02000 ELSE BEGIN KEY[4]←6;
02100 KEY[5]←IF ABS(T[85+2])<ABS(T[85+10]) THEN 2 ELSE 10
02200 END;
02300 KEY[6]←IF ABS(T[85+4])>ABS(T[85+0])
02400 THEN IF ABS(T[85+8])>ABS(T[85+4]) THEN 9 ELSE 5
02500 ELSE IF ABS(T[85+8])>ABS(T[85+0]) THEN 9 ELSE 1;
02600 MMOVE(U1[SQAR(6)],A[SQAR(6)]);
02700 FOR I←5 STEP -1 UNTIL 2 DO
02800 BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
02900 MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
03000 END;
03100 A[3]←A[7]←0.0;
03200 MMOVE(A[0],A[0]);
03300 MTIMES(U1[0],A[0],U1[SQAR(2)]);
03400 MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
03500 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
03600 MTIMES(U1[0],Q[0],U1[0]);
03700 FOR I←1 STEP 1 UNTIL 6 DO
03800 FOR J←1 STEP 1 UNTIL 6 DO
03900 NR[J,I]←U1[SQAR(I)+KEY[J]];
04000 DECOMPOSE(6,NR,LU);
04100 SOLVE(6,LU,DIR,DTH);
04200 IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
04300 MAX←0;
04400 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DIR[I]))>MAX THEN MAX←R;
04500 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DTH[I]))>MAX THEN
04600 BEGIN FOR J←I+1 STEP 1 UNTIL 6 DO IF ABS(DTH[I]+DTH[J])/R<0.5 THEN
04700 BEGIN DTH[I]←0;
04800 DTH[J]←0;
04900 GO TO OUT;
05000 END;
05100 END;
05200 OUT: FOR I←1 STEP 1 UNTIL 6 DO BEGIN
05300 DTH[I]←IF I=3 THEN DTH[I] ELSE RAD*DTH[I];
05400 NOT_LESS(DTH[I],@-5);
05500 END;
05600 UNDERFLOW(0);
05700 END;
05800
05900 INTEGER SIMPLE PROCEDURE LIMITS(SAFE REAL ARRAY JOINT);
06000 BEGIN INTEGER I;
06100 FOR I←1 STEP 1 UNTIL 5 DO
06200 IF (STOP[I,1]-JOINT[I])*(JOINT[I]-STOP[I,2])<0.0 THEN RETURN(I);
06300 RETURN (0);
06400 END;
06500
06600 SIMPLE PROCEDURE FAST_WRITE;
06700 IF FAST
06800 THEN BEGIN LABEL OK;
06900 START_CODE LABEL WRT;
07000 HRRZ 1,GOODIE;
07100 HRRZ 2,COEFF;
07200 MOVEM 2,(1);
07300 MOVE 3,BAND_NUMBER;
07400 MOVEI 4,10;
07500 WRT: UFBWRT 3,(1);
07600 SOJGE 4,WRT;
07700 MOVEI 2,'20;
07800 ADDM 2,2(1);
07900 JUMPGE 4,OK;
08000 END;
08100 USERERR(0,1,"TRAJECTORY WRITE ERROR");
08200 OK: END
08300 ELSE ARRYOUT('16,COEFF[0],'1000);
08400
08500 SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV);
08600 BEGIN
08700 STRING S;
08800 INTEGER I,L;
08900 REAL XS,YS;
09000 SAFE OWN REAL ARRAY F,M,P[1:4];
09100 SAFE OWN REAL ARRAY INV[1:4,1:4];
09200 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY R;REFERENCE REAL A;REAL ARRAY V);
09300 EXTERNAL SIMPLE PROCEDURE INVERT(REFERENCE REAL R,A);
09400 SIMPLE PROCEDURE COLVECT(REAL ARRAY R;INTEGER L,I);
09500 BEGIN INTEGER K;
09600 FOR K←1 STEP 1 UNTIL 3 DO R[K]←A[L+(K-1)*4+(I-1)];
09700 R[4]←1.0;
09800 END;
09900
10000 PUSH_FORMAT(9,1);
10100 ARRBLT(F[1],FV[1],3);
10200 ARRBLT(M[1],FV[4],3);
10300 INVERT(INV[1,1],T[SQAR(6)]);
10400 F[4]←M[4]←0.0;
10500 TRANSFORM(F,INV[1,1],F);
10600 TRANSFORM(M,INV[1,1],M);
10700 A[3]←A[7]←0.0;
10800 FOR L←6 STEP -1 UNTIL 1 DO
10900 BEGIN
11000 F[4]←M[4]←0.0;
11100 TRANSFORM (F,A[SQAR(L)],F);
11200 COLVECT(P,SQAR(L),4);
11300 F[4]←1.0;
11400 CROSS(P,P,F);
11500 TRANSFORM(M,A[SQAR(L)],M);
11600 M[4]←1.0;
11700 PLUS(M,M,P);
11800 REDUCE(M);
11900 TQ[L]←IF L=3 THEN F[3] ELSE M[3];
12000 END;
12100 A[3]←A[3]+SHOLDER[1];
12200 A[7]←A[7]+SHOLDER[2];
12300 S←"JOINT TORQUES";
12400 FOR I←1 STEP 1 UNTIL 6 DO S←S&(CVF(TQ[I]));
12500 IF TYP_HAND THEN OUTSTR(S&CRLF&CRLF);
12600 POP_FORMAT;
12700 END;
12800
12900
13000 PRELOAD_WITH
13100 '400000200000,
13200 '200000040000,
13300 '100000010000,
13400 '40000002000,
13500 '20000000400,
13600 '10000000100;
13700 SAFE OWN INTEGER ARRAY GO_WORD[1:6];
13800 SIMPLE PROCEDURE BITS(REFERENCE INTEGER DATWD;REAL SIGN;REFERENCE INTEGER DAT);
13900 START_CODE
14000 MOVE 3,-3('17);
14100 MOVE 1,(3);
14200 MOVE 2,@-1('17);
14300 TDO 1,2;
14400 TRZ 1,(2);
14500 LSH 2,1;
14600 SKIPGE -2('17);
14700 TRO 1,(2);
14800 MOVEM 1,(3);
14900 END;
15000
15100 PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD);
15200 BEGIN INTEGER NF,I,J,K;
15300 REAL MAX;
15400 SAFE OWN INTEGER ARRAY ZF[1:6];
15500 SAFE OWN REAL ARRAY FV,TQ[1:6];
15600 NF←FREE_ARM[0,1];
15700 FOR I←1 STEP 1 UNTIL NF DO
15800 BEGIN
15900 ARRBLT(FV[1],FREE_ARM[I,1],6);
16000 FORCE(TQ,FV);
16100 MAX←0.0;
16200 FOR J←1 STEP 1 UNTIL 6 DO
16300 BEGIN LABEL L1;
16400 FOR K←1 STEP 1 UNTIL I-1 DO IF ZF[K]=J THEN GO TO L1;
16500 IF ABS(TQ[J]/F0[J])>MAX THEN
16600 BEGIN
16700 MAX←ABS(TQ[J]/F0[J]);
16800 ZF[I]←J;
16900 END;
17000 L1: END;
17100 BITS(DATWD,TQ[ZF[I]],GO_WORD[ZF[I]]);
17200 END;
17300 END;
17400
00100 SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH);
00200 BEGIN
00300 LABEL F2;
00350 REAL M,IXX,IYY,IZZ;
00400 SAFE OWN REAL ARRAY TQ[1:6];
00500 EXTERNAL REAL SIMPLE PROCEDURE INNER(REFERENCE REAL A,B);
00600 REAL Z; INTEGER I,DIFF,J,K,L,TJ;
00700 M←9.33*OBJECT_MASS+M6;
00800 Z←(M6*Z6)/M;
00900 IXX←I6XX+9.33*OBJECT_MASS*OBJECT_KXX↑2;
01000 IYY←I6YY+9.33*OBJECT_MASS*OBJECT_KYY↑2;
01100 IZZ←I6ZZ+9.33*OBJECT_MASS*OBJECT_KZZ↑2;
01200 I←SQAR(6);
01300 JMAT[I]←0.5*(-IXX+IYY+IZZ);
01400 JMAT[I+5]←0.5*(IXX-IYY+IZZ);
01500 JMAT[I+10]←0.5*(IXX+IYY-IZZ);
01600 JMAT[I+11]←JMAT[I+14]←M*Z;
01700 JMAT[I+15]←M;
01800 COMPLETA(1,TH);
01900 COMPLETA(2,TH);
02000 A[SQAR(3)+11]←TH[3];
02100 COMPLETA(4,TH);
02200 COMPLETA(5,TH);
02300 COMPLETA(6,TH);
02400 UNDERFLOW(-1);
02500 RES[1]←0;ARRBLT(RES[2],RES[1],11);
02700 MTIMES(U1[0],Q[0],A[0]);
02800 MTIMES(D[0],U1[0],JMAT[0]);
02900 RES[7]←RES[7]+TRACET(D[0],U1[0]);
03000 MMOVE(T[0],A[0]);
03100 for J ← 2 step 1 until 6 do begin
03200 TJ←SQAR(J);
03300 DIFF←IF J=3 THEN 17 ELSE 0;
03400 MTIMES (QA[0], Q[DIFF], A[TJ]);
03500 MTIMES (U1[TJ], T[SQAR(J-1)], QA[0]);
03600 for I← 1 step 1 until J-1 do MTIMES (U1[SQAR(I)],U1[SQAR(I)], A[TJ]);
03700 MTIMES(T[TJ],T[SQAR(J-1)],A[TJ]);
03800 F2: FOR I←1 STEP 1 UNTIL J DO RES[I]←RES[I]
03900 +0.107*INNER(U1[SQAR(I)+8],JMAT[TJ+12]);
04000 For K← 1 step 1 until J do begin
04100 MTIMES (D[0],U1[SQAR(K)],JMAT[TJ]);
04200 RES[K+6]←RES[K+6] + TRACET(D[0],U1[SQAR(K)]);
04300 END;
04400 END;
04500 T[88]←T[88]+SX;T[92]←T[92]+SY;
04600 FOR I←1 STEP 1 UNTIL 6 DO IF FORCE_ARM[I] THEN BEGIN
04700 FORCE(TQ,FORCE_ARM);
04800 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←RES[I]+TQ[I];
04900 DONE;
05000 END;
05100 FOR I←1 STEP 1 UNTIL 12 DO NOT_LESS(RES[I],1.0@-10);
05200 I←'770000252502+(IF NNUL THEN '1000000 ELSE 0);
05300 FREE_JOINT(I);
05400 ARRBLT(RES[13],I,1);
05500 UNDERFLOW(0);
05600 END;
00100 SIMPLE PROCEDURE FLUSH(BOOLEAN FINISH;SAFE REAL ARRAY TS);
00200 BEGIN INTEGER I;
00300 IF ¬FRST_OPEN ∨ FINISH
00400 THEN BEGIN
00500 IFC WAVE THENC
00600 FOR I←1 STEP 1 UNTIL FREEL
00700 DO FOR J←1 STEP 1 UNTIL PTR3
00800 DO IF EQU(REF[J],LABELS[I])
00900 THEN BEGIN
01000 START_CODE
01100 MOVE 1,STACK;
01200 ADD 1,J;
01300 HRRE 1,-1(1);
01400 MOVEM 1,N END;
01500 N←PTRS[I]-J+N;
01600 REF[J]←NULL;
01700 IF N+J<1 ∨ N+J>PTR3+1
01800 THEN BEGIN
01900 OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
02000 N←PTR3+1-J END;
02100 STACK[J]←(N LAND '777777) LOR '102000000;
02200 LABEL_LINE[I]←NULL END;
02300 FOR I←1 STEP 1 UNTIL PTR3
02400 DO IF LENGTH(REF[I])
02500 THEN BEGIN OUTSTR(CODE_LINE[I]&REF[I]&" UNDEFINED"&'15&'12);
02600 STACK[I]←(PTR3+1-I) LOR '102000000;
02700 REF[I]←NULL;
02800 LABEL_LINE[I]←NULL END;
02900 FREEL←0;
03000 FOR I←1 STEP 1 UNTIL PTR3 DO
03100 IF (STACK[I] LAND '777000000)='102000000 THEN BEGIN
03200 START_CODE
03300 MOVE 1,STACK;
03400 ADD 1,I;
03500 HRRE 1,-1(1);
03600 MOVEM 1,N END;
03700 IF ¬N ∨ N+I<1 ∨ N+I>PTR3+1 THEN BEGIN
03800 OUTSTR(CODE_LINE[I]&"JUMP OUT OF RANGE"&CRLF);
03900 STACK[I]←(PTR3+1-I) LOR '102000000 END;
04000 END;
04100 FOR I←1 STEP 1 UNTIL CHAN DO LLAB[I]←1;
04200 ENDC
04300 PTR3←PTR3+1;
04400 STACK[PTR3]←0;
04500 IF PTR1+PTR3+PTR4≥PTR2 THEN USERERR(0,1,"TRAJECTORY FILE TOO LONG");
04600 ARRBLT(COEFF[PTR1+1],STACK[1],PTR3);
04700 FOR I←1 STEP 1 UNTIL PTR3 DO IF RELOC[I] THEN COEFF[PTR1+I]←COEFF[PTR1+I]+PTR3+PTR1;
04800 ARRBLT(COEFF[PTR1+PTR3+1],DATA_STACK[1],PTR4);
04900 PTR1←-((PTR1+1) LOR (DURATION LSH 18));
05000 ARRBLT(COEFF[0],PTR1,1);
05100 COEFF['1000]←0;
05200 PTR1←PTR4←PTR3←0;
05300 RELOC[1]←0;ARRBLT(RELOC[2],RELOC[1],99);
05400 FAST_WRITE;
05500 END ELSE FRST_OPEN←FALSE;
05600 IF FINISH THEN BEGIN
05700 IF ¬FAST THEN RELEASE('16);
05800 RETURN END;
05900 COEFF[0]←0;ARRBLT(COEFF[1],COEFF[0],'1037);
06000 ARRTRAN(TH,TS);
06100 SCHEINMAN(DIAG,TH);
06200 ARRBLT(COEFF[PTR2←499],DIAG[1],13);
06300 END;
06400
00100 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
00200 BEGIN
00300 SAFE OWN REAL ARRAY OP,A,T[1:4];
00400 UNIT(O,O);
00500 SCALE(OP,O,DOT(P,O));
00600 DIFFERENCE(A,P,OP);
00700 CROSS(T,O,A);
00800 SCALE(T,T,SIND(TH));
00900 SCALE(P,A,COSD(TH));
01000 PLUS(P,P,T);
01100 PLUS(P,P,OP);
01200 REDUCE(P);
01300 END;
00100 EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTR;INTEGER PERIOD;REFERENCE REAL BUF);
00200 SIMPLE PROCEDURE PACK_UP;BEGIN
00300 INTEGER P1,I,J,K;
00400 REAL R1,R2;
00500 SAFE OWN INTEGER ARRAY LOOPP,NXT,ALT[1:6];
00600 SAFE OWN REAL ARRAY BUF[0:4];
00700 PTR1←0;
00800 BUF[1]←0;BUF[2]←0;
00900 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01000 BUF[0]←POSITION[JOINT,0];
01100 BUF[3]←KOE[JOINT,1];BUF[4]←KOE[JOINT,2];
01200 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,1],BUF[0]);
01300 NXT[JOINT]←PTR1 END;
01400 COEFF[PTR1←PTR1+1]←PTR2←486;
01500 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01600 BUF[4]←0;
01700 K←IF LOOP THEN 7 ELSE NS[JOINT]-1;
01800 FOR I←2 STEP 1 UNTIL K DO BEGIN
01900 BUF[0]←POSITION[JOINT,I-1];
02000 FOR J←1 STEP 1 UNTIL 3 DO BUF[J]←KOE[JOINT,(3*I)-4+J];
02100 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,I],BUF[0]);
02200 COEFF[NXT[JOINT]]←(PTR1 LSH 18) LOR COEFF[NXT[JOINT]];
02300 NXT[JOINT]←PTR1;
02400 IF LOOP THEN BEGIN
02500 IF I=3 THEN LOOPP[JOINT]←PTR1;
02600 IF I=6 THEN ALT[JOINT]←PTR1 END;
02700 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←PTR2←PTR2-13 END;
02800 BUF[0]←POSITION[JOINT,I-1];
02900 BUF[1]←-4.0*(R1←KOE[JOINT,(3*I)-2])+3.0*(R2←KOE[JOINT,(3*I)-3]);
03000 BUF[2]← 6.0*R1-3.0*R2;
03100 BUF[3]←-4.0*R1 +R2;
03200 BUF[4]← R1;
03300 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,I],BUF[0]);
03400 COEFF[NXT[JOINT]]←(PTR1 LSH 18) LOR COEFF[NXT[JOINT]];
03500 BUF[4]←0;
03600 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←PTR2←PTR2-13;
03700 IF LOOP THEN BEGIN
03800 BUF[0]←POSITION[JOINT,6];
03900 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I-2+J];
04000 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,3],BUF[0]);
04100 NXT[JOINT]←PTR1;
04200 COEFF[P1←ALT[JOINT]]←(PTR1 LSH 27) LOR COEFF[P1];
04300 IF JOINT=6 THEN COEFF[P1+1]←'100000000000 LOR (LOOP LSH 18) LOR COEFF[P1+1];
04400 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←499;
04500 BUF[0]←POSITION[JOINT,0];
04600 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I+2+J];
04700 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,3],BUF[0]);
04800 COEFF[NXT[JOINT]]←(PTR1 LSH 18) LOR COEFF[NXT[JOINT]];
04900 COEFF[PTR1]←(LOOPP[JOINT] LSH 18) LOR COEFF[PTR1];
05000 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←473 END END;
05100 STACK[PTR3←PTR3+1]←'777777000000;
05200 END;
05300
00100 IFC WAVE THENC
00200 SIMPLE PROCEDURE STEP_ARM(INTEGER JOINT;REAL DTH;INTEGER TIME);
00300 BEGIN
00400 INTEGER I;
00500 IF ARM_EXECUTE THEN RETURN;
00600 FLUSH(0,LAST_ARM);
00700 FOR I←1 STEP 1 UNTIL 6 DO
00800 BEGIN
00900 POSITION[I,0]←LAST_ARM[I];
01000 NS[I]←2;
01100 PERIOD[I,1]←10;
01200 PERIOD[I,2]←IF TIME>60 THEN TIME-10 ELSE 50;
01300 IF I=JOINT THEN POSITION[I,1]←POSITION[I,2]←LAST_ARM[I]←LAST_ARM[I]+DTH
01400 ELSE POSITION[I,1]←POSITION[I,2]←LAST_ARM[I];
01500 END;
01600 COEFF[511]←GO_WORD[JOINT]+2;
01700 ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
01800 ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
01900 KOE[1,1]←0;
02000 ARRBLT(KOE[1,2],KOE[1,1],6*((MAX_SEG*3)+6)-1);
02100 PACK_UP;
02200 DURATION←10;
02300 END;
02400
02500 ENDC
02600
02700 PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5;
02800 SAFE REAL ARRAY OSHOOT[1:6];
02900
03000 BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFF;REFERENCE REAL T,D);
03100 RETURN(A4 ∧ 0+OFF< (T←-(3*A3)/(4*A4)) <1+OFF
03200 ∧ ABS(D←(IF(D←-A3*T↑3/4)*DELTA<0 THEN DELTA+D ELSE D))>OSHOOT[JOINT]);
03300
00100 SIMPLE PROCEDURE POLY(INTEGER N);BEGIN
00200 SAFE OWN REAL ARRAY A,LU[1:(3*MAX_SEG)+6,1:(3*MAX_SEG)+6];
00300 SAFE OWN REAL ARRAY B[1:(3*MAX_SEG)+6];
00400 INTEGER I,J,K,P1,N3;
00500 REAL T,T2;
00600 SIMPLE PROCEDURE THREE(INTEGER I,J;REAL T);
00700 BEGIN
00800 FOR K←J STEP 1 UNTIL J+2 DO A[I+2,K]←1.0;
00900 A[I,J]←-T;
01000 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
01100 A[I+4,J+2]←6.0*T2;
01200 A[I+3,J+2]←3.0*T;
01300 A[I+3,J+1]←2.0*T;
01400 A[I+3,J]←T;
01500 END;
01600
01700 SIMPLE PROCEDURE FOUR(INTEGER I,J;REAL T);
01800 BEGIN
01900 FOR K←J STEP 1 UNTIL J+3 DO A[I+2,K]←1.0;
02000 A[I,J]←-T;
02100 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
02200 A[I+4,J+2]←6.0*T2;
02300 A[I+4,J+3]←12.0*T2;
02400 A[I+3,J+3]←4.0*T;
02500 A[I+3,J+2]←3.0*T;
02600 A[I+3,J+1]←2.0*T;
02700 A[I+3,J]←T;
02800 END;
02900
03000 UNDERFLOW(-1);
03100 N3←N*3;
03200 T←1.0/TIM[1];
03300 T2←T*T;
03400 A[1,1]←A[1,2]←1.0;
03500 A[2,1]←3.0*T;
03600 A[2,2]←4.0*T;
03700 A[3,1]←6.0*T2;
03800 A[3,2]←12.0*T2;
03900 FOR J←1 STEP 1 UNTIL N-2 DO
04000 THREE((3*J)-1,(3*J),1.0/TIM[J+1]);
04100 A[N3-4,N3-3]←-3.0*(T←1.0/TIM[N]);
04200 A[N3-4,N3-2]←4.0*T;
04300 A[N3-3,N3-3]←6.0*(T2←T*T);
04400 A[N3-3,N3-2]←-12.0*T2;
04500 A[N3-2,N3-3]←1.0;
04600 A[N3-2,N3-2]←-1.0;
04700 FOR J←1 STEP 1 UNTIL N DO
04800 B[(J*3)-2]←POS[J]-POS[J-1];
04900 P1←N3-2;
05000 IF LOOP THEN BEGIN
05100 FOUR(N3-1,N3-1,(T←1/TIM[N-2]));
05200 FOUR(N3+2,N3+3,T);
05300 FOR J←6,7,8 DO BEGIN
05400 A[N3+5,J]←A[5,J];
05500 A[N3+6,J]←A[6,J] END;
05600 FOR J←N3-9 STEP 1 UNTIL N3-7 DO BEGIN
05700 A[N3-1,J]←A[N3-7,J];
05800 A[N3,J]←A[N3-6,J] END;
05900 B[N3+1]←B[N3-2]+B[N3-5];
06000 B[N3+4]←B[1]+B[4];
06100 P1←P1+8;
06200 END;
06300 DECOMPOSE(P1,A,LU);
06400 SOLVE(P1,LU,B,SOLN);
06500 IMPROVE(P1,A,LU,B,SOLN,T2);
06600 UNDERFLOW(0);
06700 END;
06800 INTEGER SIMPLE PROCEDURE QUADROOT(REAL A0,A1,A2,A3;REFERENCE REAL ANG,T);BEGIN
06900 REAL DISC;
07000 INTEGER I;
07100 IF A3 ∧ (DISC←(A2/(3*A3))↑2-A1/(3*A3))≥0 THEN DISC←SQRT(DISC) ELSE RETURN(0);
07200 T←DISC-A2/(3*A3);
07300 FOR I←0,1 DO BEGIN
07400 IF 0< T <1.0 THEN
07500 BEGIN
07600 ANG←(((A3*T)+A2)*T+A1)*T+A0;
07700 IF ANG>STOP[JOINT,2] THEN BEGIN ANG←STOP[JOINT,2];RETURN(1) END;
07800 IF ANG<STOP[JOINT,1] THEN BEGIN ANG←STOP[JOINT,1];RETURN(1) END;
07900 END;
08000 T←-(T+2*A2/3*A3);
08100 END;
08200 RETURN(0);
08300 END;
08400
00100 INTEGER SIMPLE PROCEDURE KISEKI;BEGIN
00200 INTEGER N,I,J,OVER;
00300 REAL ANG,T;
00400 SIMPLE PROCEDURE BUMP_UP(INTEGER I;REAL ANG,T);
00500 BEGIN INTEGER J;
00600 OVER←OVER+1;
00700 FOR J←MAX_SEG STEP -1 UNTIL I+1 DO BEGIN
00800 POSITION[JOINT,J]←POSITION[JOINT,J-1];
00900 PERIOD[JOINT,J]←PERIOD[JOINT,J-1] END;
01000 POSITION[JOINT,I]←ANG;
01100 PERIOD[JOINT,I]←T*PERIOD[JOINT,I];
01150 IF PERIOD[JOINT,I]<1 THEN PERIOD[JOINT,I]←1;
01200 PERIOD[JOINT,I+1]←PERIOD[JOINT,I+1]-PERIOD[JOINT,I] END;
01300 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01400 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
01500 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
01600 POLY(N←NS[JOINT]);
01700 OVER←0;
01800 IF OVERSHOOT(SOLN[1],SOLN[2],POS[1]-POS[0],0,T,ANG)
01900 THEN BUMP_UP(2,POS[1]+ANG,(1.0-T)*TIM[1]/TIM[2]);
02000 IF OVERSHOOT(SOLN[J←(N-1)*3],SOLN[J+1],POS[N-1]-POS[N],-1.0,T,ANG)
02100 THEN BUMP_UP(N+OVER-1,POS[N-1]+ANG,1.0-(1.0+T)*TIM[N]/TIM[N-1]);
02200 IF OVER THEN BEGIN
02300 NS[JOINT]←N+OVER;
02400 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
02500 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
02600 POLY(NS[JOINT])END;
02700 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END;
02800 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,1];
02900 SCHEINMAN(DIAG,TH);
03000 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
03100 IF NS[6]=5 ∨ (NS[6]=4 ∧ PERIOD[6,2]≤PERIOD[6,3]) THEN ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
03200 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]-1];
03300 SCHEINMAN(DIAG,TH);
03400 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
03500 IF NS[6]=5 ∨ (NS[6]=4 ∧ PERIOD[6,2]>PERIOD[6,3]) THEN ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
03600 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]];
03700 SCHEINMAN(DIAG,TH);
03800 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
03900 COMMENT THIS IS WHERE THE FORCE OPTIMIZATION GOES;
04000 FOR JOINT←2 STEP 1 UNTIL 5 DO BEGIN
04100 OVER←0;
04200 FOR I←NS[JOINT]-1 STEP -1 UNTIL 2 DO
04300 IF QUADROOT(POSITION[JOINT,I-1],KOE[JOINT,(I*3)-3],
04400 KOE[JOINT,(I*3)-2],KOE[JOINT,(I*3)-1],ANG,T)
04500 THEN BUMP_UP(I,ANG,T);
04600 NS[JOINT]←NS[JOINT]+OVER;
04700 IF OVER THEN BEGIN
04800 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
04900 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
05000 POLY(NS[JOINT]);
05100 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END END;
05200 COMMENT THIS IS WHERE THE COLLISION AVOIDER GOES;
05300 PACK_UP;
05400 RETURN(0) END;
05500
05600 PROCEDURE RESET_CONO;
05700 BEGIN
05800 FREE_ARM[0,1]←FORCE_ARM[1]←OBJECT_MASS←OBJECT_KXX←OBJECT_KYY←OBJECT_KZZ←0;
05850 NNUL←0;
05900 ARRBLT(FORCE_ARM[2],FORCE_ARM[1],5);
06000 END;
06100
06200
00100 MP SIMPLE PROCEDURE TRAJECTORY(REAL ARRAY T0,TF);BEGIN"MARK1"
00200 INTEGER MT,I,J,N;
00300 REAL INT,R;
00400 SAFE OWN REAL ARRAY AI,AF[1:6];
00500 INTEGER TR1,TR2;
00600 SIMPLE PROCEDURE LIFTOFF(INTEGER SPEED;REFERENCE REAL F;SAFE REAL ARRAY TH,DTH,DIR;REFERENCE INTEGER N,T1);
00700 BEGIN INTEGER I,J;
00800 LABEL L1,L2;
00900 SAFE OWN REAL ARRAY TT[1:6];
01000 F←1.0;
01100 IF LIMITS(TH) THEN BEGIN
01200 FOR I←1 STEP 1 UNTIL 5 DO DTH[I]←
01300 IF TH[I]≥STOP[I,2] THEN STOP[I,2]-TH[I]
01400 ELSE IF TH[I]≤STOP[I,1] THEN STOP[I,1]-TH[I]
01500 ELSE 0;
01600 GO TO L2;
01700 END;
01800 HANDPOS(TH);
01900 INCREMENT(DTH,DIR);
02000 L1: FOR I←1 STEP 1 UNTIL 6 DO TT[I]←TH[I]+2*DTH[I];
02100 I←LIMITS(TT);
02200 IF I THEN
02300 BEGIN FOR J←1,2 DO
02400 IF (TT[I]-STOP[I,J])*(STOP[I,J]-TH[I])≥0
02500 THEN
02600 BEGIN F←0.9*(STOP[I,J]-TH[I])/(TT[I]-TH[I]);
02700 DTH[I]←DTH[I]*F;
02800 DONE;
02900 END;
03000 GO TO L1;
03100 END;
03200 L2: N←IF T1>20 THEN T1 ELSE 20;
03300 FOR I←1 STEP 1 UNTIL 6 DO IF(J←SPEED*SQRT(ABS(DTH[I])/MA[I]))>N THEN N←J;
03400 END;
03500
03600 WHILE TF[6]-T0[6]>180.0 DO TF[6]←TF[6]-360.0;
03700 WHILE TF[6]-T0[6]≤-180.0 DO TF[6]←TF[6]+360.0;
03800 FLUSH(0,T0);
03900 LIFTOFF(3,F1_ARM,TH,AI,DEPART_ARM,TR1,T1_ARM);
04000 LIFTOFF(6,F2_ARM,TF,AF,ARRIVE_ARM,TR2,T2_ARM);
04100 IF DEB_HAND THEN OUTSTR("TIMES");
04200 IF (N←TR1+TR2) < 60 THEN N←60;
04300 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
04400 IF (J←2*SQRT(ABS(TH[I]-TF[I])/MA[I]))>N THEN N←J;
04500 IF DEB_HAND THEN OUTSTR(CVS(J)) END;
04600 IF DEB_HAND THEN OUTSTR(CRLF);
04700 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
04800 POSITION[I,0]←T0[I];
04900 POSITION[I,1]←T0[I]+AI[I];
05000 POSITION[I,2]←TF[I]+AF[I];
05100 COMMENT POSITION[I,2]←IF (INT←AI[I]/TR1+AF[I]/TR2) ∧ MT←ABS(INT)>1.0@-4
05200 ∧ 0 < (MT←(POSITION[I,3]-POSITION[I,1]+AF[I]*N/TR2)/INT)<N
05300 THEN AI[I]*MT/TR1+POSITION[I,1]
05400 ELSE (POSITION[I,1]+POSITION[I,3]+INT*N/2)/2;
05500 POSITION[I,3]←TF[I];
05600 NS[I]←3;
05700 PERIOD[I,1]←TR1;
05800 COMMENT PERIOD[I,2]←IF 0<MT<N THEN MT ELSE N/2;
05900 PERIOD[I,2]←N;
06000 PERIOD[I,3]←TR2 END;
06100 LOOP←0;
06200 IF KISEKI THEN USERERR(0,1,"OVERSHOOT");
06300 DURATION←N;
06400 ARRTRAN(LAST_ARM,TF);
06500 ARM_TIME←ARM_TIME+N+TR1+TR2;
06600 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←DEPART_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
06700 ARRIVE_ARM[4]←DEPART_ARM[4]←1.0;
06800 T1_ARM←T2_ARM←20;
06900 RESET_CONO;
07000 END"MARK1";
00100 MP SIMPLE PROCEDURE DRAW_ARM(INTEGER ARRAY CONTROL;REAL ARRAY PROFILE);
00200 BEGIN
00300 SAFE OWN REAL ARRAY TRANS[1:4,1:4];
00400 SAFE OWN REAL ARRAY FAS[0:6,1:6];
00500 SAFE OWN REAL ARRAY ROTS,ROTA,RS,IP,RV,CV,FVV,FV,VT,DIFF[1:4];
00600 REAL AR,RR,THP4,R,MAX;
00700 INTEGER TIME,NF,I,J,K,NT;
00800 EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY R;REFERENCE REAL A);
00900 IF ARM_EXECUTE THEN BEGIN CONTROL[1]←4;ARM_EXECUTE←FALSE;RETURN END;
01000 ARRTRAN(TRANS,LAST_TRANS);
01100 CVV(IP,LAST_TRANS,4);
01200 ARRTRAN(FAS,FREE_ARM);
01300 MOVEV(DIFF,PROFILE[1,1]);
01400 MOVEV(RV,PROFILE[2,1]);
01500 IF(AR←IF MAGNITUDE(RV) THEN PROFILE[3,1] ELSE 0) THEN UNIT(RV,RV);
01600 MOVEV(ROTA,PROFILE[5,1]);
01700 IF(RR←IF MAGNITUDE(ROTA) THEN PROFILE[3,2] ELSE 0)
01800 THEN BEGIN UNIT(ROTA,ROTA);
01900 MOVEV(ROTS,PROFILE[4,1]);
02000 DIFFERENCE(IP,IP,ROTS) END;
02100 FVV[1]←FV[1]←FORCE_ARM[1];
02200 FVV[2]←FV[2]←FORCE_ARM[2];
02300 FVV[3]←FV[3]←FORCE_ARM[3];
02400 FVV[4]←FV[4]←1.0;
02500 FLUSH(0,LAST_ARM);
02600 FRST_OPEN←TRUE;
02700 R←0;
02800 IF CONTROL[3] THEN LOOP←CONTROL[3]-1;
02900 IF LOOP THEN NT←8 ELSE BEGIN
03000 NT←4;
03100 R←MAGNITUDE(DIFF);
03200 IF R>15.0 THEN NT←(R/5)+0.5;
03300 NT←IF (MAX←AR/60.0)>NT THEN MAX ELSE NT;
03400 NT←IF (MAX←RR/60.0)>NT THEN MAX ELSE NT END;
03500 IF TYP_HAND THEN OUTSTR(CVS(NT)&" PART TRAJECTORY"&CRLF);
03600 IF NT>8 THEN BEGIN CONTROL[1]←3;RETURN END;
03700 FOR J←1 STEP 1 UNTIL 6 DO POSITION[J,0]←LAST_ARM[J];
03800 TH[6]←LAST_ARM[6];
03900 FOR I←1 STEP 1 UNTIL NT DO
04000 BEGIN
04100 R←IF I=1
04200 THEN 1/(3*(NT-2))
04300 ELSE (I-1)/(NT-2);
04400 R←IF I=NT-1
04500 THEN R-1/(3*(NT-2))
04600 ELSE IF I=NT
04700 THEN 1.0
04800 ELSE R;
04900 SCALE(VT,DIFF,R);
05000 PLUS(VT,VT,IP);
05100 IF RR THEN BEGIN ROTATE(RS,ROTS,ROTA,R*RR);
05200 PLUS(VT,VT,RS)END;
05300 REDUCE(VT);
05400 CVC(TRANS,4,VT);
05500 IF AR THEN FOR J←1 STEP 1 UNTIL 3 DO BEGIN
05600 CVV(CV,LAST_TRANS,J);
05700 REVOLVE(CV,RV,R*AR);
05800 CVC(TRANS,J,CV) END;
05900 THP4←TH[4];
06000 ARM_SOLVE(TRANS,TH,CONTROL[1]);
06100 IF ¬CONTROL[1] THEN BEGIN CONTROL[1]←1;RETURN END;
06200 IF ABS(THP4-TH[4])>90.0 THEN BEGIN CONTROL[1]←2;RETURN END;
06300 FOR J←1 STEP 1 UNTIL 6 DO POSITION[J,I]←TH[J];
06400 IF RR THEN BEGIN
06500 FOR K←1 STEP 1 UNTIL FAS[0,1] DO
06600 BEGIN
06700 MOVEV(VT,FAS[K,1]);
06800 VT[4]←1.0;
06900 IF MAGNITUDE(VT)>0.0 THEN
07000 BEGIN
07100 REVOLVE(VT,ROTA,R*RR);
07200 REDUCE(VT);
07300 FOR J←1 STEP 1 UNTIL 3 DO FREE_ARM[K,J]←VT[J] END END;
07400 MOVEV(FV,FVV[1]);
07500 REVOLVE(FV,ROTA,R*RR);
07600 REDUCE(FV);
07700 ARRBLT(FORCE_ARM[1],FV[1],3) END;
07800 SCHEINMAN(DIAG,TH);
07900 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
08000 END;
08100 FOR I←1 STEP 1 UNTIL 6 DO IF(J←2*SQRT(ABS(TH[I]-LAST_ARM[I])/MA[I]))>CONTROL[2] THEN CONTROL[2]←J;
08200 TIME←(CONTROL[2]/NT)+0.5;
08300 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
08400 NS[JOINT]←NT;
08500 FOR J←1 STEP 1 UNTIL NT DO PERIOD[JOINT,J]←TIME;
08600 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
08700 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
08800 POLY(NS[JOINT]);
08900 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+6) END;
09000 PACK_UP;
09100 DURATION←TIME;
09200 ARRTRAN(LAST_ARM,TH);
09300 ARRTRAN(LAST_TRANS,TRANS);
09400 ARM_TIME←ARM_TIME+TIME*((NT-2)*LOOP+2);
09500 CONTROL[1]←0;
09600 RESET_CONO;
09700 FRST_OPEN←FALSE;
09900 END;
00100 BOOLEAN SIMPLE PROCEDURE IS_NOT_CLEAR(SAFE REAL ARRAY TRANS,J);
00200 BEGIN SAFE OWN REAL ARRAY IP[1:4];
00300 INTEGER FLAG;
00400 REAL R;
00500 BOOLEAN SIMPLE PROCEDURE NOT_CLEAR(REFERENCE REAL J;SAFE REAL ARRAY V);
00600 BEGIN SAFE OWN REAL ARRAY VT[1:4];
00700 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY V;REFERENCE REAL T;REAL ARRAY F);
00800 TRANSFORM(VT,J,V);
00900 IF VT[3]<0.375 THEN RETURN(TRUE);
01000 IF (VT[1]-SHOLDER[1])↑2+(VT[2]-SHOLDER[2])↑2<25.0 THEN RETURN(TRUE) ELSE RETURN(FALSE);
01100 END;
01200 IP[1]←IP[3]←0.0;IP[4]←1.0;
01300 FOR R←-2.0,2.0 DO BEGIN IP[2]←R;IF NOT_CLEAR(TRANS[1,1],IP)THEN RETURN(TRUE);END;
01400 J[6]←LAST_ARM[6];
01500 ARM_SOLVE(TRANS,J,FLAG);
01600 IF ¬FLAG THEN RETURN(TRUE);
01700 HANDPOS(J);
01800 IP[2]←0.0;
01900 FOR R←3.0,-3.0 DO BEGIN IP[3]←R;IF NOT_CLEAR(T[SQAR(4)],IP) THEN RETURN(TRUE);END;
02000 IP[3]←-38.0;
02100 IF NOT_CLEAR(T[SQAR(3)],IP) THEN RETURN(TRUE) ELSE RETURN(FALSE);
02200 END;
02300
00100 MP SIMPLE PROCEDURE START_TRAJECTORY(STRING FILE;INTEGER START);
00200 BEGIN
00300 STRING S;
00400 LABEL FIND,NONE,FOUND;
00500 REAL R;
00600 FRST_OPEN←TRUE;
00700 ARM_TIME←0;
00800 IF ¬START THEN ARRTRAN(LAST_ARM,ARM_VECTOR);
00900 IF (R←LAST_ARM[6])<0 THEN R←R-360;
01000 LAST_ARM[6]←LAST_ARM[6]-((R+180) DIV 360)*360;
01100 HANDPOS(LAST_ARM);
01200 ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
01300 IF FAST THEN BEGIN
01400 FIND: IF NEXT_BAND>'37 THEN GO TO NONE;
01500 FOR BAND←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[BAND])THEN GO TO FOUND;
01600 TRACK[BAND]←CALL('100000000000 LOR BAND,"UFBGET");
01700 IF ¬_SKIP_ THEN GO TO NONE;
01800 NEXT_BAND←BAND+1;
01900 BANDS[BAND]←FILE;
02000 FOUND: GOODIE[3]←0;
02100 BAND_NUMBER←TRACK[BAND];
02200 END ELSE NONE:BEGIN
02300 FAST←FALSE;
02400 OPEN('16,"DSK",'17,0,0,120,BREAK,EOF);
02500 ENTER('16,FILE&".TRJ",BREAK);
02600 END;
02700 COEFF[0]←0; ARRBLT(COEFF[1],COEFF[0],'1037);
02800 RELOC[1]←0;ARRBLT(RELOC[2],RELOC[1],99);
02900 PTR1←PTR4←PTR3←0;
03000 PTR2←512;
03100 END;
03200
03300 MP SIMPLE PROCEDURE CLOSE_TRAJECTORY;
03400 FLUSH(1,TH);
03500
03600 MP SIMPLE PROCEDURE ARM_POSITION;
03700 BEGIN
03800 ARMPOS;
03900 UPDATE_SEG;
04000 END;
04100
00100 MP SIMPLE PROCEDURE OPEN_HAND(REAL OPENING);
00200 IF ARM_EXECUTE
00300 THEN BEGIN ARM_MESSAGE[1]←'1000000 LOR (OPENING LSH -18);
00400 HANDFN;
00500 ARM_EXECUTE←FALSE;
00600 UPDATE_SEG END
00700 ELSE STACK[PTR3←PTR3+1]←'1000000 LOR (OPENING LSH -18);
00800
00810 MP SIMPLE PROCEDURE WOBBLE_HAND(REAL WOBBLEING);
00821 IF ARM_EXECUTE
00832 THEN BEGIN ARM_MESSAGE[1]←'14000000 LOR (WOBBLEING LSH -18);
00843 HANDFN;
00854 ARM_EXECUTE←FALSE;
00865 UPDATE_SEG END
00876 ELSE STACK[PTR3←PTR3+1]←'14000000 LOR (WOBBLEING LSH -18);
00887
00900 MP SIMPLE PROCEDURE PLACE_ARM;
01000 BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
01100 INTEGER I;
01200 FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=3 THEN -0.03 ELSE 0;
01300 DIR[4]←1.0;
01400 INCREMENT(DTH,DIR);
01500 IF ARM_EXECUTE
01600 THEN BEGIN ARM_MESSAGE[7]←'4000000;
01700 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
01800 ARMFN(6);
01900 ARM_EXECUTE←FALSE;
02000 UPDATE_SEG END
02100 ELSE BEGIN STACK[PTR3←PTR3+1]←'4000000+(PTR4←PTR4+1);
02200 RELOC[PTR3]←-1;
02300 ARRBLT(DATA_STACK[PTR4],DTH[1],6);
02400 PTR4←PTR4+5 END;
02500 END;
02600
02700
02800 MP SIMPLE PROCEDURE PARK_ARM;
02850 BEGIN SAFE OWN REAL ARRAY DTH[1:6],EXF[1:13];
02875 INTEGER K,TIME;
02900 IF ¬ARM_EXECUTE THEN
03000 BEGIN
03100 IF TYP_HAND THEN PMAT("TRAJECTORY FROM",LAST_TRANS);
03200 HANDPOS(V0);
03300 ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
03400 ARRIVE_ARM[3]←0.0;
03500 IF TYP_HAND THEN PMAT("TRAJECTORY TO",LAST_TRANS);
03600 TRAJECTORY(LAST_ARM,V0);
03700 END ELSE BEGIN
03750 ARM_EXECUTE←FALSE;
03752 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←V0[I]-ARM_VECTOR[I];
03756 ARM_MESSAGE[21]←'5000000;
03760 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
03764 SCHEINMAN(EXF,V0);
03768 ARRBLT(ARM_MESSAGE[7],EXF[1],13);
03772 TIME←20;
03776 RESET_CONO;
03780 FOR I←1 STEP 1 UNTIL 6 DO IF(K←6*SQRT(ABS(DTH[I])/MA[I]))>TIME THEN TIME←K;
03784 ARM_MESSAGE[20]←TIME;
03788 ARMFN(20);
03796 UPDATE_SEG END
03798 END;
03800
03900 MP SIMPLE PROCEDURE WAIT_ARM;
04000 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'3000000 ELSE ARM_EXECUTE←FALSE;
04100
04200 MP SIMPLE PROCEDURE CLOSE_HAND(REAL DIST);
04300 IF ARM_EXECUTE
04400 THEN BEGIN ARM_MESSAGE[1]←'2000000 LOR (DIST LSH -18);
04500 HANDFN;
04600 ARM_EXECUTE←FALSE;
04700 UPDATE_SEG END
04800 ELSE STACK[PTR3←PTR3+1]←'2000000 LOR (DIST LSH -18);
04900
00100 MP SIMPLE PROCEDURE CHANGE_ARM(REAL ARRAY DIR;REAL DIST;
00200 REAL ARRAY AXIS;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00300 BEGIN INTEGER J,N;
00400 SAFE OWN REAL ARRAY AV,TQ,DTH[1:6];
00500 SAFE OWN REAL ARRAY EXF[1:13];
00600 SAFE OWN REAL ARRAY TT[1:4,1:4];
00700 SAFE OWN REAL ARRAY VT[1:4];
00800 IF ARM_EXECUTE
00900 THEN BEGIN ARRBLT(TT[1,1],T[85],16);
01000 ARRBLT(AV[1],ARM_VECTOR[1],6) END
01100 ELSE BEGIN ARRTRAN(TT,LAST_TRANS);
01200 ARRTRAN(AV,LAST_ARM)END;
01300 SCALE(VT,DIR,DIST);
01400 REDUCE(VT);
01500 FOR J←1 STEP 1 UNTIL 3 DO TT[J,4]←TT[J,4]+VT[J];
01600 IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
01700 UNIT(AXIS,AXIS);
01800 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
01900 CVV(VT,TT,I);
02000 REVOLVE(VT,AXIS,DEG);
02100 CVC(TT,I,VT) END;
02200 END;
02300 DTH[6]←AV[6];
02400 ARM_SOLVE(TT,DTH,FLAG);
02600 IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
02700 FOR I←1 STEP 1 UNTIL 6
02800 DO BEGIN DTH[I]←DTH[I]-AV[I];
02900 AV[I]←DTH[I]+AV[I] END;
03000 IF ARM_EXECUTE
03100 THEN BEGIN ARM_MESSAGE[21]←'5000000;
03200 ARRBLT(ARM_MESSAGE[1],DTH[1],6) END
03300 ELSE BEGIN ARRTRAN(LAST_ARM,AV);
03310 IF PTR1+PTR4+PTR3+25>PTR2 THEN FLUSH(0,TT);
03400 STACK[PTR3←PTR3+1]←'5000000+(PTR4←PTR4+1);
03500 RELOC[PTR3]←-1;
03600 ARRBLT(DATA_STACK[PTR4],DTH[1],6) END;
03700 SCHEINMAN(EXF,AV);
03800 IF ARM_EXECUTE
03900 THEN ARRBLT(ARM_MESSAGE[7],EXF[1],13)
04000 ELSE BEGIN ARRBLT(DATA_STACK[PTR4←PTR4+6],EXF[1],13);
04100 PTR4←PTR4+12 END;
04200 RESET_CONO;
04300 FOR I←1 STEP 1 UNTIL 6 DO IF(J←8*SQRT(ABS(DTH[I])/MA[I]))>TIME THEN TIME←J;
04400 IF ARM_EXECUTE
04500 THEN BEGIN ARM_MESSAGE[20]←TIME;
04600 ARMFN(20);
04700 ARM_EXECUTE←FALSE;
04800 UPDATE_SEG END
04900 ELSE BEGIN DATA_STACK[PTR4←PTR4+1]←TIME;
05000 ARM_TIME←ARM_TIME+TIME;
05100 ARRTRAN(LAST_TRANS,TT);
05200 FLAG←IF IS_NOT_CLEAR(LAST_TRANS,DTH)THEN 0 ELSE -1 END;
05300 END;
05400
00100 MP SIMPLE PROCEDURE CENTER_HAND(REAL DIST);
00200 BEGIN INTEGER J,N;
00300 SAFE OWN REAL ARRAY DIR[1:4];
00400 SAFE OWN REAL ARRAY DTH[1:6];
00500 IF ARM_EXECUTE THEN FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←ARM_LINK[6,J,2]
00600 ELSE FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←LAST_TRANS[J,2]; DIR[4]←1;
00700 INCREMENT(DTH,DIR);
00800 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←DTH[I]/100.0;
00900 IF ARM_EXECUTE
01000 THEN BEGIN ARM_MESSAGE[8]←'12000000;
01100 ARRBLT(ARM_MESSAGE[1],DIST,1);
01200 ARRBLT(ARM_MESSAGE[2],DTH[1],6);
01300 ARMFN(7);
01400 ARM_EXECUTE←FALSE;
01500 UPDATE_SEG END
01600 ELSE BEGIN STACK[PTR3←PTR3+1]←'12000000+(PTR4←PTR4+1);
01700 RELOC[PTR3]←-1;
01800 ARRBLT(DATA_STACK[PTR4],DIST,1);
01900 ARRBLT(DATA_STACK[PTR4←PTR4+1],DTH[1],6);
02000 PTR4←PTR4+5 END;
02100 END;
02200
02300 MP SIMPLE PROCEDURE SET_ARM(INTEGER CELL;REAL ARRAY TRANS,DX_DY_DZ,AXIS;
02400 REAL DEG;REFERENCE INTEGER FLAG);
02500 BEGIN SAFE OWN REAL ARRAY VT[1:4],DTH,AV[1:6];
03900 ARM_SOLVE(TRANS,AV,FLAG);
04000 IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
05900 REDUCE(DX_DY_DZ);
06000 FOR J←1 STEP 1 UNTIL 3 DO TRANS[J,4]←TRANS[J,4]+DX_DY_DZ[J];
06100 IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
06200 UNIT(AXIS,AXIS);
06300 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
06400 CVV(VT,TRANS,I);
06500 REVOLVE(VT,AXIS,DEG);
06600 CVC(TRANS,I,VT) END;
06700 END;
06800 DTH[6]←AV[6];
06850 FLAG←FALSE;
06900 IF IS_NOT_CLEAR(TRANS,DTH)THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
07000 FLAG←TRUE;
07200 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←DTH[I]-AV[I];
07300 IF ARM_EXECUTE THEN BEGIN
07400 ARM_MESSAGE[8]←'13000000;
07450 ARM_MESSAGE[1]←CELL;
07500 ARRBLT(ARM_MESSAGE[2],DTH[1],6);
07600 ARMFN(7);
07700 ARM_EXECUTE←FALSE;
07800 UPDATE_SEG END
07900 ELSE BEGIN
08000 STACK[PTR3←PTR3+1]←'13000000+(PTR4←PTR4+1);
08100 RELOC[PTR3]←-1;
08150 DATA_STACK[PTR4]←CELL;
08200 ARRBLT(DATA_STACK[PTR4←PTR4+1],DTH[1],6);
08300 PTR4←PTR4+5 END;
08500 END"SET_ARM";
08600
00100 MP SIMPLE PROCEDURE DRIVE_ARM(INTEGER JOINT;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00200 BEGIN INTEGER I,J,N;
00300 REAL R;
00400 SAFE OWN REAL ARRAY EXF[1:13];
00500 SAFE OWN REAL ARRAY LA,TQ[1:6];
00600 FLAG←FALSE;
00700 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
00800 R←LA[JOINT]+DEG;
00900 IF JOINT<6 ∧ (STOP[JOINT,1]-R)*(R-STOP[JOINT,2])<1 THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
01000 FLAG←TRUE;
01100 LA[JOINT]←R;
01200 SCHEINMAN(EXF,LA);
01300 IF ARM_EXECUTE
01400 THEN BEGIN ARM_MESSAGE[21]←'5000000;
01500 ARM_MESSAGE[1]←0;ARRBLT(ARM_MESSAGE[2],ARM_MESSAGE[1],5);
01600 ARRBLT(ARM_MESSAGE[JOINT],DEG,1) END
01700 ELSE BEGIN ARRBLT(LAST_TRANS[1,1],T[85],16);
01800 STACK[PTR3←PTR3+1]←'5000000+(PTR4←PTR4+1);
01900 RELOC[PTR3]←-1;
02000 DATA_STACK[PTR4]←0;
02100 ARRBLT(DATA_STACK[PTR4+1],DATA_STACK[PTR4],5);
02200 ARRBLT(DATA_STACK[PTR4+JOINT-1],DEG,1) END;
02300 N←IF TIME>20 THEN TIME ELSE 20;
02400 IF (J←6*SQRT(ABS(DEG)/MA[JOINT]))>N THEN N←J;
02500 IF (J←ABS(DEG)/3)>N THEN N←J;
02600 IF ARM_EXECUTE
02700 THEN BEGIN ARRBLT(ARM_MESSAGE[7],EXF[1],13);
02800 ARM_MESSAGE[19]←GO_WORD[JOINT]+2;
02900 IF FREE_ARM[0,1] THEN FREE_JOINT(ARM_MESSAGE[19]);
03000 ARM_MESSAGE[20]←N;
03100 ARMFN(20);
03200 ARM_EXECUTE←FALSE;
03300 UPDATE_SEG END
03400 ELSE BEGIN ARRBLT(DATA_STACK[PTR4←PTR4+6],EXF[1],13);
03500 PTR4←PTR4+12;
03600 DATA_STACK[PTR4]←GO_WORD[JOINT]+2;
03700 IF FREE_ARM[0,1] THEN FREE_JOINT(DATA_STACK[PTR4]);
03800 DATA_STACK[PTR4←PTR4+1]←N;
03900 ARM_TIME←ARM_TIME+N;
04000 ARRTRAN(LAST_ARM,LA) END;
04100 RESET_CONO;
04200 END;
04300
04400 MP SIMPLE PROCEDURE STOP_ARM(REAL ARRAY F,M;REFERENCE INTEGER STAT);
04500 BEGIN SAFE OWN REAL ARRAY TQ,XF[1:6];
04600 LABEL OK;
04700 INTEGER I;
04800 HANDPOS(LAST_ARM);
04900 REDUCE(F);
05000 ARRBLT(XF[1],F[1],3);
05100 REDUCE(M);
05200 ARRBLT(XF[4],M[1],3);
05300 FORCE(TQ,XF);
05400 FOR I←1 STEP 1 UNTIL 6 DO IF ABS(TQ[I]/F0[I])≥0.2 THEN GO TO OK;
05500 STAT←0;
05600 RETURN;
05700 OK: STAT←-1;
05800 STACK[PTR3←PTR3+1]←'7000000+(PTR4←PTR4+1);
05900 RELOC[PTR3]←-1;
06000 ARRBLT(DATA_STACK[PTR4],TQ[1],6);
06100 PTR4←PTR4+5;
06200 END;
06300
06400 MP SIMPLE PROCEDURE MOVE_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
06500 BEGIN SAFE OWN REAL ARRAY DTH,J[1:6];
06510 SAFE OWN REAL ARRAY EXF[1:13];
06520 INTEGER K,TIME;
06600 FLAG←0;
06700 IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;;
06705 FLAG←-1;
06710 IF ARM_EXECUTE THEN BEGIN
06712 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←J[I]-ARM_VECTOR[I];
06726 ARM_MESSAGE[21]←'5000000;
06730 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
06750 SCHEINMAN(EXF,J);
06758 ARRBLT(ARM_MESSAGE[7],EXF[1],13);
06770 TIME←20;
06772 RESET_CONO;
06774 FOR I←1 STEP 1 UNTIL 6 DO IF(K←6*SQRT(ABS(DTH[I])/MA[I]))>TIME THEN TIME←K;
06780 ARM_MESSAGE[20]←TIME;
06786 ARMFN(20);
06790 ARM_EXECUTE←FALSE;
06794 UPDATE_SEG END
06797 ELSE BEGIN
06800 IF TYP_HAND THEN PMAT("TRAJECTORY FROM",LAST_TRANS);
06900 ARRTRAN(LAST_TRANS,T);
07000 IF TYP_HAND THEN PMAT("TRAJECTORY TO",T);
07100 TRAJECTORY(LAST_ARM,J) END;
07300 END;
00100 MP SIMPLE PROCEDURE DO_IT(STRING FILE);
00200 BEGIN INTEGER I;
00300 ARM_STATUS←0;
00400 ARM_EXECUTE←FALSE;
00500 FOR I←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[I])THEN DONE;
00600 DOIT(TRACK[I],CVSIX(FILE));
00700 IF ARM_STATUS='50 ∧TRACK[I] THEN BANDS[I]←NULL;
00800 UPDATE_SEG;
00900 END;
01000
01100 MP SIMPLE PROCEDURE MERGE_ARM;
01200 BEGIN
01300 IF ARM_EXECUTE ∨ PTR3<2 ∨ STACK[PTR3-1]≠'777777000000 THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;;
01400 STACK[PTR3-1]←(((LNOT STACK[PTR3]) LAND '777777000000) LOR (STACK[PTR3] LAND '777777));
01500 PTR3←PTR3-1;
01600 END;
01700
01800 MP SIMPLE PROCEDURE DO_PROCEED;
01900 BEGIN SAFE OWN REAL ARRAY DTH[1:6];
02000 ARM_EXECUTE←FALSE;
02100 IF ¬ARM_WAIT THEN RETURN;
02200 ARMPROCEED;
02300 UPDATE_SEG;
02400 END;
02500
02600 MP SIMPLE PROCEDURE ARM_CONO(REAL ARRAY ARRIVE,DEPART,OBJECT;INTEGER ARRIVE_TIME,DEPART_TIME);
02700 BEGIN ARRTRAN(ARRIVE_ARM,ARRIVE);
02800 ARRTRAN(DEPART_ARM,DEPART);
02810 OBJECT_MASS←OBJECT[4];
02820 OBJECT_KXX←OBJECT[1];
02830 OBJECT_KYY←OBJECT[2];
02840 OBJECT_KZZ←OBJECT[3];
02900 T1_ARM←DEPART_TIME;
03000 T2_ARM←ARRIVE_TIME;
03100 END;
03200
03300 MP SIMPLE PROCEDURE SET_TOUCH(BOOLEAN STOP_ON_TOUCH);
03400 IF ¬ARM_EXECUTE THEN
03500 STACK[PTR3←PTR3+1]←'6000000+(IF STOP_ON_TOUCH THEN '777777 ELSE 0)
03600 ELSE ARM_EXECUTE←FALSE;
03700
03800 MP SIMPLE PROCEDURE ARM_SKIPE(INTEGER I);
03900 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'101000000 LOR I ELSE ARM_EXECUTE←FALSE;
04000
04100 MP SIMPLE PROCEDURE ARM_SKIPN(INTEGER I);
04200 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'103000000 LOR I ELSE ARM_EXECUTE←FALSE;
04300
04400 MP SIMPLE PROCEDURE ARM_SKIPS(INTEGER I);
04500 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'104000000 LOR I ELSE ARM_EXECUTE←FALSE;
04600
04700 MP SIMPLE PROCEDURE ARM_JMP(INTEGER I);
04800 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'102000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
04900
05000 MP SIMPLE PROCEDURE ARM_SAVE(INTEGER LEVEL);
05100 IF ARM_EXECUTE
05200 THEN BEGIN ARM_MESSAGE[1]←'10000000+LEVEL;
05300 HANDFN;
05400 ARM_EXECUTE←FALSE;
05500 UPDATE_SEG END
05600 ELSE STACK[PTR3←PTR3+1]←'10000000+LEVEL;
05700
05800 MP SIMPLE PROCEDURE ARM_RESTORE(INTEGER LEVEL);
05900 IF ARM_EXECUTE
06000 THEN BEGIN ARM_MESSAGE[1]←'11000000+LEVEL;
06100 HANDFN;
06200 ARM_EXECUTE←FALSE;
06300 UPDATE_SEG END
06400 ELSE STACK[PTR3←PTR3+1]←'11000000+LEVEL;
06500